#include "Process.h"
using namespace std;
using namespace cv;


void Filter2d(Mat& src, Mat& Kernel, Mat* dstX, Mat* dstY)
{
	//动态核心长度的梯度提取、图像经过平滑之后固定核心Canny无法提取到有效梯度。需要增加卷积的范围
	int sz = Kernel.cols / 2;
	int Ksz = Kernel.cols - 1;
	Mat Interpolate;
	copyMakeBorder(src, Interpolate, sz, sz, sz, sz, BORDER_REPLICATE);

	*dstX = Mat(src.size(), CV_8UC1);
	*dstY = Mat(src.size(), CV_8UC1);
	size_t colStart = sz;
	size_t colEnd = AlignLo(Interpolate.cols - Ksz, 32);
	size_t rowStart = sz;
	size_t rowEnd = Interpolate.rows - sz;

	bool hasEnd = colEnd != (Interpolate.cols - Ksz);
	size_t Signm = Interpolate.cols - Ksz - 16;

	//初始化核心参数
	__m256i* KVal = new __m256i[sz];
	for (size_t i = 0; i < sz; i++)
	{
		KVal[i] = _mm256_set1_epi16(-Kernel.at<short>(0, i));
	}

	__m256i M128 = _mm256_set1_epi16(128);	//核心扩大了128倍、转换8位灰度图时需要除以128
	__m256i MA;							    //前后相减结果
	__m256i MSUMA;
	__m256i MSUMB;
	__m256i ValA, ValB;						//两排结果的中间变量   （同时处理两排、减少写数据的IO访问）
	///*************************************求X方向梯度**************************************************
	for (size_t i = rowStart; i < rowEnd; i++)
	{
		uchar* ptr = Interpolate.ptr<uchar>(i);
		uchar* ptrDis = dstX->ptr<uchar>(i - sz);
		for (size_t j = 0; j < colEnd; j += 32)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			uchar* addsa = ptr + Ksz + j;
			uchar* addsb = ptr + j;
			for (size_t k = 0; k < sz; k++)//自适应不同长度核心
			{
				const __m256i LDA = _mm256_loadu_epi8(addsa - k);
				const __m256i LDB = _mm256_loadu_epi8(addsb + k);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			ValA = _mm256_div_epi16(_mm256_abs_epi16(MSUMA), M128); //转换位8位灰度
			ValB = _mm256_div_epi16(_mm256_abs_epi16(MSUMB), M128); //转换位8位灰度
			_mm256_storeu_epi8(ptrDis + j, _mm256_permute4x64_epi64(_mm256_packus_epi16(ValA, ValB), 0xD8));
		}
		//存在非32的整数倍时
		if (hasEnd)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(ptr + Ksz - k + Signm);
				const __m256i LDB = _mm256_loadu_epi8(ptr + k + Signm);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			ValA = _mm256_div_epi16(_mm256_abs_epi16(MSUMA), M128); //转换位8位灰度
			ValB = _mm256_div_epi16(_mm256_abs_epi16(MSUMB), M128); //转换位8位灰度
			_mm256_storeu_epi8(ptrDis + Signm, _mm256_permute4x64_epi64(_mm256_packus_epi16(ValA, ValB), 0xD8));
		}
	}
	//**************************************求Y方向梯度**************************************************
	colEnd = AlignLo(Interpolate.cols - sz, 32);
	rowEnd = Interpolate.rows - Ksz;
	hasEnd = colEnd != (Interpolate.cols - Ksz);
	Signm = Interpolate.cols - Ksz - 32;
	for (size_t j = sz; j < colEnd; j += 32)
	{
		for (size_t i = 0; i < rowEnd; i++)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i - k + Ksz, j));
				const __m256i LDB = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i + k, j));
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			ValA = _mm256_div_epi16(_mm256_abs_epi16(MSUMA), M128); //转换位8位灰度
			ValB = _mm256_div_epi16(_mm256_abs_epi16(MSUMB), M128); //转换位8位灰度
			_mm256_storeu_epi8(dstY->ptr<uchar>(i, j - sz), _mm256_permute4x64_epi64(_mm256_packus_epi16(ValA, ValB), 0xD8));
		}
	}
	//存在非16的整数倍时
	if (hasEnd)
	{
		for (size_t i = 0; i < rowEnd; i++)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i - k + Ksz, Signm));
				const __m256i LDB = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i + k, Signm));
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			ValA = _mm256_div_epi16(_mm256_abs_epi16(MSUMA), M128); //转换位8位灰度
			ValB = _mm256_div_epi16(_mm256_abs_epi16(MSUMB), M128); //转换位8位灰度
			_mm256_storeu_epi8(dstY->ptr<uchar>(i, Signm - sz), _mm256_permute4x64_epi64(_mm256_packus_epi16(ValA, ValB), 0xD8));
		}
	}
	delete[] KVal;
}

void Filter2d16(Mat& src, Mat& Kernel, Mat* dstX, Mat* dstY)
{
	//动态核心长度的梯度提取、图像经过平滑之后固定核心Canny无法提取到有效梯度。需要增加卷积的范围
	int sz = Kernel.cols / 2;
	int Ksz = Kernel.cols - 1;
	Mat Interpolate;
	copyMakeBorder(src, Interpolate, sz, sz, sz, sz, BORDER_REPLICATE);

	*dstX = Mat(src.size(), CV_16SC1);
	*dstY = Mat(src.size(), CV_16SC1);
	size_t colStart = sz;
	size_t colEnd = AlignLo(Interpolate.cols - Ksz, 32);
	size_t rowStart = sz;
	size_t rowEnd = Interpolate.rows - sz;

	bool hasEnd = colEnd != (Interpolate.cols - Ksz);
	size_t Signm = Interpolate.cols - Ksz - 16;

	//初始化核心参数
	__m256i* KVal = new __m256i[sz];
	for (size_t i = 0; i < sz; i++)
	{
		KVal[i] = _mm256_set1_epi16(-Kernel.at<short>(0, i));
	}

	__m256i M128 = _mm256_set1_epi16(128);	//核心扩大了128倍、转换8位灰度图时需要除以128
	__m256i MA;							    //前后相减结果
	__m256i MSUMA;
	__m256i MSUMB;
	__m256i ValA, ValB;						//两排结果的中间变量   （同时处理两排、减少写数据的IO访问）
	///*************************************求X方向梯度**************************************************
	for (size_t i = rowStart; i < rowEnd; i++)
	{
		uchar* ptr = Interpolate.ptr<uchar>(i);
		ushort* ptrDis = dstX->ptr<ushort>(i - sz);
		for (size_t j = 0; j < colEnd; j += 32)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			uchar* addsa = ptr + Ksz + j;
			uchar* addsb = ptr + j;
			for (size_t k = 0; k < sz; k++)//自适应不同长度核心
			{
				const __m256i LDA = _mm256_loadu_epi8(addsa - k);
				const __m256i LDB = _mm256_loadu_epi8(addsb + k);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			_mm256_storeu_epi16(ptrDis + j, _mm256_abs_epi16(MSUMA));
			_mm256_storeu_epi16(ptrDis + j+16, _mm256_abs_epi16(MSUMB));
		}
		//存在非32的整数倍时
		if (hasEnd)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(ptr + Ksz - k + Signm);
				const __m256i LDB = _mm256_loadu_epi8(ptr + k + Signm);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			_mm256_storeu_epi16(ptrDis + Signm, _mm256_abs_epi16(MSUMA));
			_mm256_storeu_epi16(ptrDis + Signm + 16, _mm256_abs_epi16(MSUMB));
		}
	}
	//**************************************求Y方向梯度**************************************************
	colEnd = AlignLo(Interpolate.cols - sz, 32);
	rowEnd = Interpolate.rows - Ksz;
	hasEnd = colEnd != (Interpolate.cols - Ksz);
	Signm = Interpolate.cols - Ksz - 32;
	for (size_t j = sz; j < colEnd; j += 32)
	{
		for (size_t i = 0; i < rowEnd; i++)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i - k + Ksz, j));
				const __m256i LDB = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i + k, j));
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			_mm256_storeu_epi16(dstY->ptr<uchar>(i, j - sz), _mm256_abs_epi16(MSUMA));
			_mm256_storeu_epi16(dstY->ptr<uchar>(i, j - sz + 16), _mm256_abs_epi16(MSUMB));
		}
	}
	//存在非16的整数倍时
	if (hasEnd)
	{
		for (size_t i = 0; i < rowEnd; i++)
		{
			MSUMA = _mm256_set1_epi16(0);
			MSUMB = _mm256_set1_epi16(0);
			for (size_t k = 0; k < sz; k++)
			{
				const __m256i LDA = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i - k + Ksz, Signm));
				const __m256i LDB = _mm256_loadu_epi8(Interpolate.ptr<uchar>(i + k, Signm));
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDA)), _mm256_cvtepu8_epi16(_mm256_castsi256_si128(LDB)));
				MSUMA = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMA);
				MA = _mm256_sub_epi16(_mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDA, 1)), _mm256_cvtepu8_epi16(_mm256_extractf128_si256(LDB, 1)));
				MSUMB = _mm256_add_epi16(_mm256_mullo_epi16(MA, KVal[k]), MSUMB);
			}
			_mm256_storeu_epi16(dstY->ptr<uchar>(i, Signm - sz), _mm256_abs_epi16(MSUMA));
			_mm256_storeu_epi16(dstY->ptr<uchar>(i, Signm - sz + 16), _mm256_abs_epi16(MSUMB));
		}
	}
}

void GenDistance(cv::Mat& srcA, cv::Mat& srcB, cv::Mat* dst)
{
#define TYPES 1
	*dst = Mat(srcA.size(), CV_16SC1);
	size_t colEnd = AlignLo(srcA.cols, 16);
	bool hasEnd = colEnd != srcA.cols;
	size_t Signm = srcA.cols - 16;
	ushort* PtrA;
	ushort* PtrB;
	ushort* PtrDst;

	for (size_t i = 0; i < srcA.rows; i++)
	{
		PtrA = srcA.ptr<ushort>(i);
		PtrB = srcB.ptr<ushort>(i);
		PtrDst = dst->ptr<ushort>(i);
		for (size_t j = 0; j < colEnd; j+=16)
		{

#if TYPES
			_mm256_storeu_epi16(PtrDst + j, _mm256_adds_epu16(_mm256_loadu_epi16(PtrA + j), _mm256_loadu_epi16(PtrB + j)));
#endif 

#if !TYPES
			const __m256i S1 = _mm256_loadu_epi16(PtrA + j);
			const __m256i S2 = _mm256_loadu_epi16(PtrB + j);
			const __m256i hi = _mm256_cvtps_epi32(_mm256_sqrt_ps(_mm256_cvtepi32_ps(_mm256_add_epi32(_mm256_mullo_epi16(S1, S1), _mm256_mullo_epi16(S2, S2)))));
			const __m256i lo = _mm256_cvtps_epi32(_mm256_sqrt_ps(_mm256_cvtepi32_ps(_mm256_add_epi32(_mm256_mulhi_epi16(S1, S1), _mm256_mulhi_epi16(S2, S2)))));
			_mm256_storeu_epi16(PtrDst + j, PackI32ToI16(lo, hi));
#endif 

		}
		if (hasEnd)
		{
#if TYPES
			_mm256_storeu_epi16(PtrDst + Signm, _mm256_adds_epu16(_mm256_loadu_epi16(PtrA + Signm), _mm256_loadu_epi16(PtrB + Signm)));
#endif 
		}
	}
}
